import lejos.nxt.*;
import lejos.robotics.navigation.*;


public class SquareDisplay {
	
	static double SIDE_SIZE = 40.0;
	static double WHEEL_DIAMETER = 5.6f;
	static double AXIS_LENGTH = 5.6f;
	
	public static void main(String[] args) throws InterruptedException {
		
		double x = 0.0, y = 0.0, zeta = 0.0;
		double D = 0;
		double alpha = 0;
		double lastTurn = 0;
		double lastDisp = 0;
		double distanceTraveled, angleTurned;
		
		//Initialize differentialpilot navigator for manage the movement
		DifferentialPilot navigator = new DifferentialPilot(WHEEL_DIAMETER, AXIS_LENGTH, Motor.A, Motor.C);
		
		//Set movement parameters
		navigator.setRotateSpeed(100);
		navigator.setTravelSpeed(100);
		navigator.setAcceleration(200);
		
		//Wait 1 second to begin
		Thread.sleep(1000);
		
		//Forward - stop - rotate
		for (int i = 0; i < 4; i++)
		{
			
			//Advance 40 cm
			navigator.travel(SIDE_SIZE, true);
			
			//Initialice actual distance traveled
			distanceTraveled = navigator.getMovement().getDistanceTraveled();
			
			lastDisp = 0;
			
			//While we are advancing
			while (navigator.getMovement().isMoving())
			{
				//Update increment of D
				D = distanceTraveled - lastDisp; 
				lastDisp = distanceTraveled;
				
				//Update x and y
				x = x + (D * Math.cos(Math.toRadians(zeta)));
				y = y + (D * Math.sin(Math.toRadians(zeta)));

				//Print coordinates
				System.out.println("("+(int)x+", "+(int)y+", "+(int)zeta+")");
				
				//Update distance traveled
				distanceTraveled = navigator.getMovement().getDistanceTraveled();
			}
			
			//Last update until begin the rotation
			D = distanceTraveled - lastDisp; 
			lastDisp = distanceTraveled;
			x = x + (D * Math.cos(Math.toRadians(zeta)));
			y = y + (D * Math.sin(Math.toRadians(zeta)));
			
			//Rotate 90 degrees
			navigator.rotate(90, true);
			
			//Update actual degree
			angleTurned = navigator.getMovement().getAngleTurned();
			
			lastTurn = 0;
			
			//While we are rotating
			while (navigator.getMovement().isMoving())
			{
				//Update increment rotation
				alpha = angleTurned - lastTurn;
				lastTurn = angleTurned;
				
				//Update zeta
				zeta = zeta + alpha;
				
				//Print coordinates and orientation
				System.out.println("("+(int)x+", "+(int)y+", "+(int)zeta+")");
				angleTurned = navigator.getMovement().getAngleTurned();
			}
			
			//Last update until next iteration
			alpha = angleTurned - lastTurn;
			lastTurn = angleTurned;
			zeta = zeta + alpha;
				
		}
		
		Button.waitForPress();
	}
}


